import numpy as np

import matplotlib.pyplot as plt
import scipy.interpolate as scipy_interpolate


def approximate_b_spline_path(x: list, y: list, n_path_points: int, s: float = 0.0,
                              degree: int = 3) -> tuple:
    """
    approximate points with a B-Spline path

    :param x: x position list of approximated points
    :param y: y position list of approximated points
    :param n_path_points: number of path points
    :param s : float, optional
        A smoothing condition.
    :param degree: (Optional) B Spline curve degree
    :return: x and y position list of the result path
    """
    t = range(len(x))
    x_tup = scipy_interpolate.splrep(t, x, s=s, k=degree)
    y_tup = scipy_interpolate.splrep(t, y, s=s, k=degree)

    x_list = list(x_tup)
    x_list[1] = x + [0.0, 0.0, 0.0, 0.0]

    y_list = list(y_tup)
    y_list[1] = y + [0.0, 0.0, 0.0, 0.0]

    ipl_t = np.linspace(0.0, len(x) - 1, n_path_points)
    rx = scipy_interpolate.splev(ipl_t, x_list)
    ry = scipy_interpolate.splev(ipl_t, y_list)

    print(len(rx), len(ry))

    return rx, ry


def interpolate_b_spline_path(x: list, y: list, n_path_points: int,
                              degree: int = 3) -> tuple:
    """
    interpolate points with a B-Spline path

    :param x: x positions of interpolated points
    :param y: y positions of interpolated points
    :param n_path_points: number of path points
    :param degree: B-Spline degree
    :return: x and y position list of the result path
    """
    ipl_t = np.linspace(0.0, len(x) - 1, len(x))
    spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree)
    spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree)

    travel = np.linspace(0.0, len(x) - 1, n_path_points)
    return spl_i_x(travel), spl_i_y(travel)


way_point_x = [-28.079998016357422, -21.060001373291016, -15.47999382019043, -9.539995193481445, 1.980010986328125,
               9.539995193481445, 19.259994506835938, 30.59999656677246, 15.480010986328125, 5.219999313354492,
               -11.879997253417969, -24.1199951171875, -30.95999526977539, -28.079998016357422]
way_point_y = [66.42001342773438, 70.0199966430664, 68.03999328613281, 67.1399917602539, 68.03999328613281,
               68.57998657226562, 69.83999633789062, 63.53999328613281, 61.20000076293945, 58.86000061035156,
               61.739990234375, 64.260009765625, 65.34001159667969, 66.42001342773438]

# way_point_x = [36.360008239746094, 38.87999725341797, 28.800006866455078, 12.060001373291016, 2.3400020599365234,
#                -8.819995880126953, -14.219999313354492, -8.460004806518555, -2.8799972534179688, 1.080007553100586,
#                8.640008926391602, 14.759994506835938, 20.52000617980957, 36.360008239746094]
# way_point_y = [63.360008239746094, 61.73999786376953, 61.73999786376953, 58.31999588012695, 60.66000747680664,
#                63.900001525878906, 67.1399917602539, 66.23999786376953, 64.97999572753906, 64.25999450683594,
#                65.33999633789062, 66.95999908447266, 67.31999206542969, 63.360008239746094]


def b_spline_path_test():
    x_lst = [0., 1.2, 1.9, 3.2, 4., 6.5]
    y_lst = [0., 2.3, 3., 4.3, 2.9, 3.1]

    x = np.array(x_lst)
    y = np.array(y_lst)

    t, c, k = scipy_interpolate.splrep(x, y, s=0.001, k=3)
    print('''\
    t: {}
    c: {}
    k: {}
    '''.format(t, c, k))
    N = 100
    xmin, xmax = x.min(), x.max()
    xx = np.linspace(xmin, xmax, N)
    spline = scipy_interpolate.BSpline(t, c, k, extrapolate=False)

    plt.plot(x, y, 'bo', label='Original points')
    plt.plot(xx, spline(xx), 'r', label='BSpline')
    plt.grid()
    plt.legend(loc='best')
    plt.show()